'''
Focal Length:          fc = [ 385.54767   395.78809 ] [ 16.74732   16.16721 ]                  
Principal point:       cc = [ 271.03355   268.93622 ]
                            [ 18.39942   34.50658 ]                         
Distortion:            kc = [ -0.04247   0.01671   0.02108   0.01492  0.00000 ]
                            [ 0.21649   0.15416   0.02690   0.03829  0.00000 ]                     
Pixel error:          err = [ 0.24704   0.19300 ]

Extrinsic parameters:
Translation vector: Tc_ext = [ -2.739371 	 -24.193550 	 79.396603 ]
Rotation vector:   omc_ext = [ -2.154992 	 -2.222297 	 0.344086 ]
Rotation array:    Rc_ext = [ -0.042385 	 0.984172 	 -0.172071
                               0.990126 	 0.018343 	 -0.138972
                               -0.133616 	 -0.176262 	 -0.975232 ]
Pixel error:           err = [ 0.29246 	 0.17048 ]
'''

import math
import numpy as np


# 1、外参数测定
class Coordinate:
    # 外参数
    Tv = np.array([-2.739371, -24.193550, 79.396603])
    Rv = np.array([-2.154992, -2.222297, 0.344086])
    Rm = np.array([[-0.042385, 0.984172, -0.172071],
                   [0.990126, 0.018343, -0.138972],
                   [-0.133616, -0.176262, -0.975232]])

    # 内参数
    u0 = 271.03355
    v0 = 268.93622
    fx = 385.54767
    fy = 395.78809
    zc = 100  # unknown

    # 图像参数 640*480
    px = 0
    py = 0
    dx = 2.54 / 96
    dy = 2.54 / 96
    uO0 = 320 * 2.54 / 96
    vO0 = 240 * 2.54 / 96

    # 畸变参数
    distortion = np.array([-0.04247, 0.01671, 0.02108, 0.01492, 0.00000])  # k1, k2, p1, p2, k3
    Intrinsic = np.array([[fx, 0, u0], [0, fy, v0], [0, 0, 1]])
    Extrinsic = np.vstack((np.hstack((Rm, Tv.reshape((3, 1)))), np.array([0, 0, 0, 1])))

    # 像素坐标转图像坐标
    def pixtopic(self, u, v):
        u = u * self.dx - self.uO0
        v = v * self.dy - self.vO0
        return u, v

    # 图像坐标转相机坐标
    def pictocam(self, u, v):
        u, v = self.eliminateDistortion(u, v)
        cam_coordinate = np.matmul(np.linalg.inv(self.Intrinsic),
                                   np.array([u, v, 1], dtype=object).reshape([3, 1])) * self.zc
        return cam_coordinate

    # 相机坐标转世界坐标
    def camtoworld(self, x, y, z):
        world_coordiante = np.matmul(np.linalg.inv(self.Extrinsic),
                                     np.array([x, y, z, 1], dtype=object).reshape([4, 1]))
        return world_coordiante

    # 消除图像畸变
    def eliminateDistortion(self, u, v):
        u0, v0 = self.pixtopic(self.u0, self.v0)
        ud, vd = u - u0, v - v0
        r = math.sqrt(math.pow(ud, 2) + math.pow(vd, 2))
        k1, k2, p1, p2, k3 = self.distortion
        uud = ud * (1 + k1 * math.pow(r, 2) + k2 * math.pow(r, 4) + k3 * math.pow(r, 6)) + 2 * p1 * ud * vd + p2 * (
                math.pow(r, 2) + 2 * math.pow(ud, 2))
        vud = vd * (1 + k1 * math.pow(r, 2) + k2 * math.pow(r, 4) + k3 * math.pow(r, 6)) + p1 * (
                math.pow(r, 2) + 2 * math.pow(vd, 2)) + 2 * p2 * ud * vd
        uud = uud + u0
        vud = vud + v0
        return uud, vud

    # 像素坐标转图像坐标
    def pixtoworld(self, u, v):
        x, y = self.pixtopic(u, v)
        cam = self.pictocam(x, y)
        world = self.camtoworld(cam[0], cam[1], self.zc)
        return world[0], world[1], world[2]

    # 由zw计算zc坐标
    def calzc(self, zw):
        zc = zw * self.Rm[0][2] + zw * self.Rm[1][2] + zw * self.Rm[2][2] + self.Tv[2]
        return zc
